/*******************************************************************************
OFFunctions.c

These are the basic optical flow functions. Ideally these will be called by
the firmware module.

Origin:
Adapted from OFSupport by Travis Young, which was adapted from libraries by
Craig Neely

Change Log:
2010 May 21, Friday, birth
2010 August 9, Monday, documentation
******************************************************************************/

// Possible changes: Lots of optimization for speed etc.

#include "OFFunctions.h"
#include "F64PlusInterface.h"
#include "ApplicationPrototype.h"



//////////////////////////////////////////////////////////////////////////////
// PIX_HPF8: High pass update using 8-bit low pass record and single-value
// update to/from the input for low pass computation
//
// This function is called:
//////////////////////////////////////////////////////////////////////////////
void Pix_HPF8(unsigned char *Iin, unsigned char *Ilp, unsigned char *Ihp, unsigned char skipcnt)
{
	unsigned char *pin = Iin;
	unsigned char *plp = Ilp;
	unsigned char *php = Ihp;
	unsigned int idx;

	// loop through


	// do the actual adaptation
	for (idx=0; idx!=PINHOLE_IMAGE_SIZE; ++idx) {
		if (*pin>*plp) {
			// adapt *plp upward
			if (skipcnt==0)		
				(*plp) ++;
			// compute high passed
			*php = *pin-*plp;
			if (*php>125)
				*php = 125;
			*php += 128;
		} else if (*pin<*plp) {
			// adapt *plp downward
			if (skipcnt==0)
				(*plp) --;
			// compute high passed
			*php = *plp-*pin;
			if (*php>125)
				*php = 125;
			*php = 128 - *php;
		} else
			*php = 128; // nominal zero
		++pin;
		++plp;
		++php;
	}
}

//////////////////////////////////////////////////////////////////////////////
// PIX_HPF: High pass update with single pole low pass
//
// At the current time, this is the function being used.
//
// This function implements a time domain high pass filter on the pixels
// of the array.
//
// This function is called:
//////////////////////////////////////////////////////////////////////////////
/*
void Pix_HPF(unsigned char *Iin, signed int *Ilp, unsigned char *Ihp, unsigned char shift, unsigned int Isize)
{	
	unsigned char	*pin, *php;					// pixel row / column indexes
	signed int		idx, *plp;					// pixel index

	//shift=16;  // gives different results, I think this actually causes Ilp to initialize differently
	// initialize pointers
	pin=Iin;
	php=Ihp;
	plp=Ilp;

	// loop through if not shifting e.g. copy over
	if (shift==0) {
		for (idx=0; idx!=Isize; ++idx) {
			*php = *pin;
			++php;
			++pin;
		}
		return;
	}

	// loop through if shifting
	for (idx=0; idx!=Isize; ++idx) {
		// low pass part
		signed int pinbig = (signed int)(*pin);
		signed int pinbigshift = pinbig<<7;
		signed int diff = pinbigshift - *plp;
		signed int diffshift = diff >> shift;
		signed int newplp = *plp + diffshift;
		*plp = newplp;
		// high pass part
		signed int pinbigshiftminusplp = pinbigshift - newplp;
		signed int thatshifted = pinbigshiftminusplp >> 7;
		*php = (unsigned char)((thatshifted+128) & 0xFF);

		pin++;
		plp++;
		php++;

		//Ihp[0]=diffshift>>8;
	    //Ihp[1]=(diffshift<<8)>>8;

		//Ihp[0]=(*Ilp)>>8;
	    //Ihp[1]=((*Ilp)<<8)>>8;
	}
	// *Ihp=shift;

}
*/

//////////////////////////////////////////////////////////////////////////////
// PIX_HPF: high pass filter
// replaces Iin in place
// It appears in the implementation above that Ihp is not actually used anywhere as an input, only as output
// In the implementation above pin, plp, and php are initialized to Iin, Ilp, and Ihp.
//
//////////////////////////////////////////////////////////////////////////////
void Pix_HPF(unsigned char *const Iin, signed int *const Ilp, const unsigned char shift)
{	
	unsigned int idx;			
	
	// initialize pointers
	unsigned char *pin=Iin;
	signed int *plp=Ilp;

	// return if not shifting
	if (shift==0) return;

	// loop through if shifting
	for (idx=0; idx!=PINHOLE_IMAGE_SIZE; ++idx) {
		
		// low pass part
		signed int pinbigshift = (signed int)(*pin)<<7;
		signed int diffshift = (pinbigshift - *plp) >> shift;
		//signed int diffshift = (pinbigshift - *plp) >> 16;
		*plp += diffshift;
		
		// high pass part
		signed int thatshifted = (pinbigshift - *plp) >> 7;
		
		*pin = (unsigned char)((thatshifted+128) & 0xFF);

		pin++;
		plp++;
	}
}

//////////////////////////////////////////////////////////////////////////////
// ADDFPN: Adds a fixed pattern noise mask- used for high-passed image
//
// This function ADDS a fixed pattern noise to the array. This can be useful
// when the pixels are high passed and there is no motion so the high 
// passing produces a virtually zero image. Noise in such an image can cause
// some optical flow algorithms to screw up. So this function can be used to
// add a small fixed pattern noise to "anchor" the image and keep optical flow
// small. Qualitatively we are trying to prevent a divide-by-zero condition.
//
// This function is called:
//////////////////////////////////////////////////////////////////////////////
void AddFPN(unsigned char *const X, const unsigned char strength)
{
	unsigned char *px1 = X,*px2;

	if (strength==0)
		return;

	for (unsigned char i=0; i<PINHOLE_IMAGE_HEIGHT; i+=4) {
		px2 = px1;
		for (unsigned char j=0; j<PINHOLE_IMAGE_WIDTH; j+=4) {
			*px2 += strength;
			px2 += 4;
		}
		px1 += PINHOLE_IMAGE_WIDTH*4;
	}
}

//////////////////////////////////////////////////////////////////////////////
// SUBTRACTCALMASK: Subtract the calibration mask from an image
//
// if we are using the calibration mask, then subtract it out
// this section might have overflow issues in some light levels
// should make this a function
//
// This function is called:
//////////////////////////////////////////////////////////////////////////////
void SubtractCalMask(unsigned char *const Iin)
{
	if (!use_image_cal)
		return;
	else {
		unsigned char *ptemp=Iin;
		unsigned int temp;
		
		for (unsigned int i=0; i<PINHOLE_IMAGE_SIZE; ++i) {
			temp = *ptemp;
			temp += 40;
			temp -= Cal.read[i];
			*ptemp = (unsigned char)temp;
			++ptemp;
		}
	}

}

//////////////////////////////////////////////////////////////////////////////
// IIA2: Basic two dimensional optical flow algorithm using IIA in 2D.
//
// Basically given two images, compute an X and Y displacement between
// them.
//
// This is the current optical flow algorithm being used.
//
// This function is called:
//////////////////////////////////////////////////////////////////////////////
void IIA2(const unsigned char *const X1, const unsigned char *const X2)
{
	int32_t	    A11=0, A12=0, A22=0, b1=0, b2=0;
	int16_t		F2F1, F4F3, FCF0;
	
	// set up pointers
	const unsigned char *f0 = X1 + PINHOLE_IMAGE_WIDTH + 1;	
	const unsigned char *f1 = X1 + PINHOLE_IMAGE_WIDTH + 2;
	const unsigned char *f2 = X1 + PINHOLE_IMAGE_WIDTH;
	const unsigned char *f3 = X1 + 2*PINHOLE_IMAGE_WIDTH + 1;
	const unsigned char *f4 = X1 + 1;
	const unsigned char *fz = X2 + PINHOLE_IMAGE_WIDTH + 1;

	// loop through
	for (unsigned char r=1; r!=PINHOLE_IMAGE_HEIGHT-1; ++r) {
		for (unsigned char c=1; c!=PINHOLE_IMAGE_WIDTH-1; ++c) {
			// compute differentials, then increment pointers (post increment)
			F2F1 = (*(f2++) - *(f1++));
			F4F3 = (*(f4++) - *(f3++));
			FCF0 = (*(fz++) - *(f0++));

			// update summations
			A11 += (F2F1 * F2F1);
			A12 += (F4F3 * F2F1);
			A22 += (F4F3 * F4F3);
			b1  += (FCF0 * F2F1);			
			b2  += (FCF0 * F4F3);					
		}
		f0+=2;
		fz+=2;
		f1+=2;
		f2+=2;
		f3+=2;
		f4+=2;
	}

	//float detA = ( (float)(A11)*A22 - (float)(A12)*A12 );
	//float XS = ( (float)(b1)*A22 - (float)(b2)*A12 ) * of_scale / detA;
	//float YS = ( (float)(b2)*A11 - (float)(b1)*A12 ) * of_scale / detA;
	
	int64_t detA = ( (int64_t)(A11)*A22 - (int64_t)(A12)*A12 );
	int64_t XS = ( (int64_t)(b1)*A22 - (int64_t)(b2)*A12 ) * of_scale / detA;
	int64_t YS = ( (int64_t)(b2)*A11 - (int64_t)(b1)*A12 ) * of_scale / detA;
	
	OFX=(signed char)(XS);
	OFY=(signed char)(YS);

}
